#include <Wire.h>
#include <stdlib.h>
#define BYTE_NUM 32
#define INT_NUM BYTE_NUM/2

#define B_SCHEDA_POTENZA_1 32
#define B_SCHEDA_POTENZA_2 32
#define B_SCHEDA_SENSORI 28
#define B_SCHEDA_MOTORI_1 32
#define B_SCHEDA_MOTORI_2 32
//----CMDS----
#define CA0 'a'
#define CA1 'b'
#define CA2 'c'
#define CA5 'd'
#define CAR 'e'
#define EXT 'x'
#define FAN '1'
#define MOF 'g'
#define MON 'h'
#define OFF 'f'
#define ON 'o'
#define POI 'n'
#define POV 'p'
#define R 'r'
#define SCA 'q'
#define STOP 's'
#define TE 't'
#define VE 'v'
#define DAT 'z'
#define STB 'y'
#define SHT 'w'
#define EMU 'i'
#define MAF 'm'
#define MLM 'l'
//-----------

#define grado 20  //gradi massimi accettabili per free wheel

int warnings_sensori[16];
int warnings_potenza[16];
int warnings_motori_reg[2][4][32]; //MF e SR
int warnings_motori[16]; 
int dati_sensori[B_SCHEDA_SENSORI/2];
int dati_potenza_1[B_SCHEDA_POTENZA_1/2];
int dati_potenza_2[B_SCHEDA_POTENZA_2/2];
int dati_motori_1[B_SCHEDA_MOTORI_1/2];
int dati_motori_2[B_SCHEDA_MOTORI_2/2];
char resp[32];

unsigned int Tc=1000,Tc_potenza=1000;
byte ca0=0,ca1=0,ca2=3,ca5=0;
byte Te=2,Ve=5;
byte req=0;
unsigned long time_count;
int pON=1;
boolean stato[7][2]={{true, true},{true, true},{true, true},{true, true},{true, true},{true, true},{true, true}}; //0=MASTER  1=POTENZA   2=MOTORI   3=RADIO    4=TELECAMERE    5=SENSORI  carica o scarica/accesa o spenta    6a=EMBEDDED   6b=FAN 
boolean sblocco=false;
boolean Mlm=false;
byte Maf=0;

//serial variable 
char serInString[3][20];  // array that will hold the different bytes  100=100characters;
                        // -> you must state how long the array will be else it won't work.
int  serInIndx  = 0;    // index of serInString[] in which to insert the next incoming byte

void setup()
  {
  byte stato_i2c;
  pinMode(9,INPUT);      // se alto non trasmettere a motori
  pinMode(13,OUTPUT);    //led notifica master (segnala lettura dati i2c in corso)
  //attachInterrupt(5, function, falling) //18 esterni
  //attachInterrupt(4, function, falling) //19 schede tutte
  pinMode(40,OUTPUT);    //opto 1
  pinMode(41,OUTPUT);    //opto 2
  pinMode(42,OUTPUT);    //opto 3
  pinMode(43,OUTPUT);    //opto 4
  pinMode(44,OUTPUT);    //stand by pc
  pinMode(45,OUTPUT);    //interrupt to motori
  pinMode(46,OUTPUT);    //led 1 (allarme master)  
  pinMode(47,OUTPUT);    //led 2 (allarme motori)-
  pinMode(48,OUTPUT);    //led 3 (allarme potenza)-
  pinMode(49,OUTPUT);    //led 4 (allarme sensori)-
  pinMode(50,OUTPUT);    //led 5 (acceso con link attivo, lampeggio per comando ricevuto)- no lampeggio
  pinMode(51,OUTPUT);    //led 6 (pc attivo)-
  pinMode(52,OUTPUT);    //led 7 (interrupt in corso)
  pinMode(53,OUTPUT);    //led 8 (perdita di un nodo i2c o problema i2c)-
  
  Wire.begin(0);        // join i2c bus (address optional for master)
  Serial.begin(115200);  // start serial for output
  Serial3.begin(115200);
  Serial3.write("Scrivi comandi\r\n");
  digitalWrite(50,HIGH); //link seriale on
  
  delay(200); //aspetta che si accendano tutti i nodi prima di iniziare a inviare comandi
  
  //inizializzazione periferiche
    //fan
    Wire.beginTransmission(1);
    Wire.write(61);
    stato[6][1]=true;
    if(Wire.endTransmission()!=0) 
      {
      digitalWrite(53,HIGH); //prob i2c
      stato[6][1]=false;
      }
    
    //embedded (spento all'avvio, ora viene acceso a mano)
//    digitalWrite(44,HIGH);
//    delay(20);
//    digitalWrite(44,LOW);
    stato[6][0]=false;
    digitalWrite(51,LOW);
    
    //MAF auto
    int WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
    if(WDT<40){Wire.beginTransmission(2); Wire.write(48);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
    Maf=6;
    if(stato_i2c!=0) 
      {
      digitalWrite(47,HIGH); //allarme motori
      digitalWrite(53,HIGH); //prob i2c
      delay(500); //se no non si vede l'allarme i2c su led
      Maf=0;
      
      //resetto la scheda motori bloccata
      Wire.beginTransmission(1);  
      Wire.write(20+2);     //OFF     
      stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
      delay(Tc_potenza+500);
      Wire.beginTransmission(1);  
      Wire.write(10+2);     //ON    
      stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
      delay(1000); //se no va in loop di reset perchè non ha tempo di accendersi
      }
    
    
    //inizializz ext
    resp[0]='*';
    resp[1]=';';
    resp[2]='*';
    resp[3]=';';
	

  
  time_count=0;
  }

void loop()
  { 
  byte i,j,k,z;
  byte stato_i2c;
  byte IDS,IDD;
  int WDT=0;
  digitalWrite(53,LOW); //resetta allarme i2c
  
  if (millis()-time_count>=Tc)
    {
    ////////  Master  ////// 
    IDS=0;
    digitalWrite(46,LOW); //resetta allarme master
    IDD=1;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(ca0);Serial.println(";");
    IDD=2;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(ca1);Serial.println(";");
    IDD=3;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(ca2);Serial.println(";");
    IDD=6;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(ca5);Serial.println(";");
    IDD=7;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(Te);Serial.println(";");
    IDD=8;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(Ve);Serial.println(";");
    IDD=9;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(stato[6][0]);Serial.println(";");
    IDD=10;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(stato[6][1]);Serial.println(";");
    IDD=11;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(pON);Serial.println(";");
	IDD=12;
    Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");
	i=0;
	j=2;
	while(j)
          {
  	  Serial.print(resp[i]);
  	  i++;
          if ((resp[i]==';')||(resp[i]=='|'))
             {
             j--;
             resp[i]='|';
             }
	  }
	Serial.println(";");
	
 
      
    
    digitalWrite(13,HIGH);
    if (pON==1)
      {
      //////  Potenza 1  //////
      IDS=1;
      digitalWrite(48,LOW); //resetta eventuale allarme potenza
      req=0; 
      Wire.beginTransmission(1);Wire.write(req);stato_i2c=Wire.endTransmission();
      if (stato_i2c==0)
        {
        int i=ricevi_i2c(dati_potenza_1,1);
        for (j=0;j<6;j++)
            {
               if (bitRead(dati_potenza_1[j],15)==1)
                 stato[j][0]=true; //ricarica
               else
                 stato[j][0]=false;
               bitClear(dati_potenza_1[j],15);
               
               if (bitRead(dati_potenza_1[j],14)==1)
                 stato[j][1]=true; //accesa       
               else
                 stato[j][1]=false;
               bitClear(dati_potenza_1[j],14);
            }
        for (j=0;j<6;j++)
            {
               IDD=j*3+1;
               Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(stato[j][0]);Serial.println(";");
               IDD=j*3+2;
               Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(stato[j][1]);Serial.println(";");
               IDD=j*3+3;
               Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_potenza_1[j]);Serial.println(";");
            }
        for (j=6; j<(i/2); j++)
          {
            IDD=j+13;
            Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_potenza_1[j]);Serial.println(";");
          }
        }
      else
        {
        digitalWrite(48,HIGH); //allarme potenza
        digitalWrite(53,HIGH); //prob i2c
        }
      
      //////  Potenza 2  //////
      req=1;
      Wire.beginTransmission(1); Wire.write(req);stato_i2c=Wire.endTransmission();
      if (stato_i2c==0)
        {
        i=ricevi_i2c(dati_potenza_2,1);
        read_warnings(warnings_potenza,dati_potenza_2[14],16);
        for (j=0; j<14; j++)
          {
            IDD=j+29;
            Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_potenza_2[j]);Serial.println(";");
          }  
        IDD=43;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");
        for (j=15+1; j>0; j--)
          {
            Serial.print(warnings_potenza[j-1]);
            if(warnings_potenza[j-1]==1)
              digitalWrite(48,HIGH); //allarme potenza
          }  
        Serial.println(";");
        
        Serial3.print("Allarmi potenza: ");
        for (j=15+1; j>0; j--)
          {
            Serial3.print(warnings_potenza[j-1]);
          }  
        Serial3.println();
        
        IDD=44;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_potenza_2[15]);Serial.println(";");
        }
      else
        {
        digitalWrite(48,HIGH); //allarme potenza
        digitalWrite(53,HIGH); //prob i2c
        }
      
      ////////  Sensori  ////// 
      IDS=3;
      //digitalWrite(40,HIGH);
      digitalWrite(49,LOW); //resetta allarme sensori
      req=0; //req fittizzio, solo per sapere se riceve
      Wire.beginTransmission(5); Wire.write(req);stato_i2c=Wire.endTransmission();
      //digitalWrite(40,LOW);
      if (stato_i2c==0)
        {  
        i=ricevi_i2c(dati_sensori,5);
        read_warnings(warnings_sensori,dati_sensori[7],16);
        for (j=0; j<7; j++)
          {
            IDD=j+1;
            Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_sensori[j]);Serial.println(";");
          }
        
        IDD=8;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");
        for (j=15+1; j>0; j--)
          {
            Serial.print(warnings_sensori[j-1]);
            if(warnings_sensori[j-1]==1)
              digitalWrite(49,HIGH); //allarme sensori
          }  
        Serial.println(";");
        
        Serial3.print("Allarmi sensori: ");
        for (j=15+1; j>0; j--)
          {
            Serial3.print(warnings_sensori[j-1]);
          }  
        Serial3.println();
          
        for (j=8; j<(i/2); j++)
          {
            IDD=j+1;
            Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_sensori[j]);Serial.println(";");
          }
          
        if((abs(dati_sensori[6])>=(1800-grado*10))&&(Maf==5))
          {
          WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
          if(WDT<40){Wire.beginTransmission(2); Wire.write(251);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
          if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c
          }
        if((dati_sensori[5]>=900)&&(Maf==6))   //pende indietro
          {
          if((dati_sensori[4]<=900)&&(Maf==6)) //pende a sinistra
            {
            WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
            if(WDT<40){Wire.beginTransmission(2); Wire.write(253);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c
            //Serial.println("---------------------------------------253");
            }
          else  //pende a destra
            {
            WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
          if(WDT<40){Wire.beginTransmission(2); Wire.write(252);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c
            //Serial.println("---------------------------------------252");
            }
          }
        else  //pende in avanti
          {
          if((dati_sensori[4]<=900)&&(Maf==6)) //pende a sinistra
            {
            WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
          if(WDT<40){Wire.beginTransmission(2); Wire.write(255);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c
            //Serial.println("---------------------------------------254");
            }
          else  //pende a destra
            {
            WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
          if(WDT<40){Wire.beginTransmission(2); Wire.write(254);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c
            //Serial.println("---------------------------------------255");
            }
          }
        
        
        if (((dati_sensori[8]>35) || (dati_sensori[8]==0) || (dati_sensori[9]>35) || (dati_sensori[9]==0) || (dati_sensori[10]>55) || (dati_sensori[10]==0)) && (stato[6][1]==false))
          {
            Wire.beginTransmission(1);
            Wire.write(61);
            stato_i2c=Wire.endTransmission();
            if  (stato_i2c!=0) 
              digitalWrite(53,HIGH); //prob i2c
            else
              {           
              stato[6][1]=true;
              }
           }
         else if (((dati_sensori[8]<34) && (dati_sensori[9]<34) && (dati_sensori[10]<50)) && (stato[6][1]==true))
           {
            Wire.beginTransmission(1);
            Wire.write(60);
            stato_i2c=Wire.endTransmission();
            if  (stato_i2c!=0) 
              digitalWrite(53,HIGH); //prob i2c
            else
              {           
              stato[6][1]=false;
              }
           }
        }
      else
        {
        digitalWrite(49,HIGH); //allarme sensori
        digitalWrite(53,HIGH); //prob i2c
        }
    
      //////  Motori 1  //////
      IDS=2;
      digitalWrite(47,LOW); //resetta allarme motori
      req=0; //richiedo il pacchetto di dati numero 0  
      WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
      if(WDT<40){Wire.beginTransmission(2); Wire.write(req);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
      if (stato_i2c==0)
        {  
        delay(60);
        i=ricevi_i2c(dati_motori_1,2);
        read_warnings(warnings_motori,dati_motori_1[15],16);
        for (j=0; j<11; j++)
        {
          IDD=j+1;
          Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(dati_motori_1[j]);Serial.println(";");
        }
        
        byte temperature[2][2];
        integerToBytes(dati_motori_1[11],temperature[0]);
        integerToBytes(dati_motori_1[12],temperature[1]);
        Serial3.print("Velocita motore 1: ");Serial3.println(dati_motori_1[3]);
        Serial3.print("Velocita motore 2: ");Serial3.println(dati_motori_1[4]);
        Serial3.print("Velocita motore 3: ");Serial3.println(dati_motori_1[5]);
        Serial3.print("Velocita motore 4: ");Serial3.println(dati_motori_1[6]);
        IDD=12;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(temperature[0][0]);Serial.println(";");
        Serial3.print("Temperatura motore 1: ");Serial3.println(temperature[0][0]);
        IDD=13;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(temperature[0][1]);Serial.println(";");
        Serial3.print("Temperatura motore 2: ");Serial3.println(temperature[0][1]);
        IDD=14;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(temperature[1][0]);Serial.println(";");
        Serial3.print("Temperatura motore 3: ");Serial3.println(temperature[1][0]);
        IDD=15;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(temperature[1][1]);Serial.println(";");
        Serial3.print("Temperatura motore 4: ");Serial3.println(temperature[1][1]);
        
        byte time[2];                                             //0 - dt dati, 1 - dt ultimo flag
        integerToBytes(dati_motori_1[13],time);
        IDD=16;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(time[0]);Serial.println(";");
        IDD=17;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(time[1]);Serial.println(";");
        
        byte flag[2];
        integerToBytes(dati_motori_1[14],flag);
        IDD=18;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(flag[0]);Serial.println(";");
        IDD=19;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");Serial.print(flag[1]);Serial.println(";");
        
        IDD=20;
        Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");
        for (j=15+1; j>0; j--)
          {
            Serial.print(warnings_motori[j-1]);
            if(warnings_motori[j-1]==1)
              digitalWrite(47,HIGH); //allarme motori
          } 
        Serial.println(";");
        
        Serial3.print("Allarmi motori: ");
        for (j=15+1; j>0; j--)
          {
            Serial3.print(warnings_motori[j-1]);
          } 
        Serial3.println();
      
        }
      else
        {
        digitalWrite(47,HIGH); //allarme motori
        digitalWrite(53,HIGH); //prob i2c
        
        //resetto la scheda motori bloccata
        Wire.beginTransmission(1);  
        Wire.write(20+2);     //OFF     
        stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
        delay(Tc_potenza+500);
        Wire.beginTransmission(1);  
        Wire.write(10+2);     //ON    
        stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
        delay(1000); //se no va in loop di reset perchè non ha tempo di accendersi
        }
      
      if(!Mlm)  //motori" lo legge solo se non ÃƒÆ’Ã‚Â¨ in modalitÃƒÆ’  rapida
        {
        //////  Motori 2  //////
        req=1; 
        delay(60);
        WDT=0; while ((digitalRead(9)==HIGH)&&(WDT<40)){delay(50); WDT=WDT+1;}  //massimo consentito 2 secondi
        if(WDT<40){Wire.beginTransmission(2); Wire.write(req);stato_i2c=Wire.endTransmission();}else{stato_i2c=1;}
        if (stato_i2c==0)
          {   
          delay(60);
          i=ricevi_i2c(dati_motori_2,2);
          for (k=0;k<4;k++)//leggi registri motore
            {
            for(j=0;j<2;j++) // MF
          	   {
          		read_warnings(&warnings_motori_reg[0][k][j*16],dati_motori_2[j+2*k],16);
          	   }
            for(j=0;j<2;j++) // SR
          	   {
          		read_warnings(&warnings_motori_reg[1][k][j*16],dati_motori_2[8+j+2*k],16);
          	   }
            }
          for (k=0; k<4; k++)
            {
              IDD=21+k;
              Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");
              for (z=31+1; z>0; z--)
                {
                Serial.print(warnings_motori_reg[0][k][z-1]); //MF/SR, motore, bit
                }
              Serial.println(";");
            }
          for (k=0; k<4; k++)
            {
              IDD=25+k;
              Serial.print(IDS);Serial.print(";");Serial.print(IDD);Serial.print(";");
              for (z=31+1; z>0; z--)
                {
                Serial.print(warnings_motori_reg[1][k][z-1]);
                }
              Serial.println(";");
            }  
          }
        else
          {
          digitalWrite(47,HIGH); //allarme motori
          digitalWrite(53,HIGH); //prob i2c
          
          //resetto la scheda motori bloccata
          Wire.beginTransmission(1);  
          Wire.write(20+2);     //OFF     
          stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
          delay(Tc_potenza+500);
          Wire.beginTransmission(1);  
          Wire.write(10+2);     //ON    
          stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
          delay(1000); //se no va in loop di reset perchè non ha tempo di accendersi
          }
        }
      else //se sono in Mlm non legge motori 2 e stampa tutti 0 al posto dei registri
        {
        IDS=2;
        for (z=21; z<29; z++)
          {
          Serial.print(IDS);Serial.print(";");Serial.print(z);Serial.print(";");Serial.println("0;");
          }
        }
       
       
       
       
        
      } //fine codice sotto POI
    time_count=millis();
    digitalWrite(13,LOW);
    } //fine codice polling
  
  
  
  
  
  
    
  delay(10);
  }





  ////////////////
 //  funzioni  //
////////////////

int ricevi_i2c(int *dato,int scheda)
  {
    int i=0;
    byte temp[BYTE_NUM];
    
    switch(scheda)
    {
  	case 1:
  	  if (req==0)
  	  {
  	    Wire.requestFrom(scheda, B_SCHEDA_POTENZA_1);
            } else if(req==1)
      	  {
      	    Wire.requestFrom(scheda, B_SCHEDA_POTENZA_2);
            }
  	  break;
          case 2:
            while (digitalRead(9)==HIGH){delay(50);}
  	  if (req==0)
    	    {
    	    Wire.requestFrom(scheda, B_SCHEDA_MOTORI_1);
              } 
            else if(req==1)
        	    {
        	    Wire.requestFrom(scheda, B_SCHEDA_MOTORI_2);
              }
  	  break;
  	case 5:
  	  Wire.requestFrom(scheda, B_SCHEDA_SENSORI);
  	  break;
    }
    while(Wire.available())    // slave may send less than requested
    { 
          temp[i] = Wire.read();
          i++;
    }
    i=i-(i%2); // se riceve un numero dispari di byte l'ultimo viene tolto  
    bytesToInteger(temp,i, dato);
    return i;
  }



void bytesToInteger(byte *b,byte ByteNumber, int *dato) 
  {
    int k=0;
    for (k=0; k<ByteNumber/2;k++)
    {  
  	  dato[k] = ((int )b[2*k]) << 8;
            dato[k] |= b[2*k+1];
    }	  
  }



void integerToBytes(int val, byte b[2]) 
  {
    b[0] = (byte ((val >> 8) & 0xff));
    b[1] = (byte(val & 0xff));
  }



void read_warnings(int *warn, int rcvd,byte num_warn)
  {
    byte i;
    for  (i=0;i<num_warn;i++)
    {
      if (bitRead(rcvd,i)) warn[i]=1;
      else warn[i]=0; 
    }
  }
  


void serialEvent() 
  {
    int sb; 
    int i=0;
    byte temp[10],stato_i2c;
    digitalWrite(50,LOW); //comando ricevuto
    boolean sicurezza;
      
    while(Serial.available()) { 
       strcpy(serInString[0],"                   ");
       strcpy(serInString[1],"                   "); 
       strcpy(serInString[2],"                   ");
       byte cont=0;
       while (Serial.available()){ 
          if (serInIndx>18) break; 
          sb = Serial.read();
          serInString[0][serInIndx] = sb;
          serInIndx++;
          serInString[0][serInIndx] = -1;
          if (sb==';') 
            cont++;
          if (cont==3)
            break;
       }
       int j=0;
       int z=0;
       while(serInString[0][j]!=';')                       //split input string in index (row 0) and command (row 1)
          j++;
       j++;
       while(serInString[0][j]!=';')
          {
          serInString[1][z]=serInString[0][j];
          j++;
          z++;
          }
       j++;
       z=0;
       while(serInString[0][j]!=';')//-1)
          {
          serInString[2][z]=serInString[0][j];
          j++;
          z++;
          }
       serInString[2][z]=';';                              //verificare se legge correttamente con o senza stop!
       int comando=atol(serInString[1]);
       int comando2;
 
       //Serial.println("||--Comando seriale ricevuto--||");
       j=0;
       while(serInString[0][j]!=';')                       //stampa il comando
          {
          //Serial.print(serInString[0][j]);  
          j++;
          }
       //Serial.println();
       //Serial.println(comando);
       if (serInString[0][0]!='E')
         {
         comando2=atol(serInString[2]);
         //Serial.println(comando2);
         }
       else
         {
         //Serial.println(serInString[2]);        
         }
       //Serial.println("||------------||||------------||");
       
 
   
       switch (serInString[0][0])
         {
          case ON:
            Wire.beginTransmission(1);  
            Wire.write(10+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c 
            break;
          
          case OFF:
            Wire.beginTransmission(1);  
            Wire.write(20+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;
          
          case CAR:
            Wire.beginTransmission(1);  
            Wire.write(30+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;
            
          case CA0:
            if (comando==0)
              Tc=1*1000;
            else if (comando==1)
              Tc=2*1000;
            else if (comando==2)
              Tc=5*1000;
            else if (comando==3)
              Tc=10*1000;
            else if (comando==4)
              Tc=30*1000;
            else if (comando==5)
              Tc=60*1000;
            ca0=comando;
            break;
          
          
          case CA1:  
            Wire.beginTransmission(1);  
            Wire.write(50+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            if (comando==0)             //eliminare tutto questo? forse col buffer va anche senza
              Tc_potenza=1*1000;
            else if (comando==1)
              Tc_potenza=2*1000;
            else if (comando==2)
              Tc_potenza=5*1000;
            else if (comando==3)
              Tc_potenza=10*1000;
            else if (comando==4)
              Tc_potenza=30*1000;
            else if (comando==5)
              Tc_potenza=60*1000;
            ca1=comando;
            break;  
              
          case CA2:
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  
            Wire.write(34+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            ca2=comando;
            break;
            
          case CA5:
            Wire.beginTransmission(5);  // transmit to device 5
            Wire.write(10+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            ca5=comando;
            break;
          
          case EXT:
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  
            j=0;
            while(serInString[2][j]!=';')                       //split input string in index (row 0) and command (row 1)
                {
                temp[j+2]=serInString[2][j];
                j++;
                }
            temp[0]=49+j;
            temp[1]=byte(comando); //motore #
            Wire.write(temp,j+2);     // sends "49+lungh","motore","...stringa..."
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
	    delay(20);
            i=0;	
            for(;;){ //infinite loop, waiting for reposnse
				
			if (digitalRead(9)==LOW){
                                Wire.beginTransmission(2);
				Wire.write(3);
				stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
                                delay(100);				
				Wire.requestFrom(2,32);
				
				while(Wire.available())    // slave may send less than requested
				{ 
					resp[i] = Wire.read();
					i++;
				}
				break; // stops infinite loop
			}
			}
            break;
          
          case SCA:
            Wire.beginTransmission(1);  
            Wire.write(40+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;
            
          case STOP:
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(20);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2cstato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;

          case R:
            Wire.beginTransmission(1);  
            Wire.write(20+comando);     //OFF     
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            delay(Tc_potenza+500);
            Wire.beginTransmission(1);  
            Wire.write(10+comando);     //ON    
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;
          
          case TE:
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(27+comando);        
            Te=comando; 
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else { Te=comando; }
          break;
          
          case VE:    
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(21+comando);
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else Ve=comando;   
          break;

          case POI:
            pON=comando;
            break;           
            
          case POV:
            while (digitalRead(9)==HIGH){delay(50);}
            
            //controllo sicurezza
            sicurezza=true;
            //over temperature motore o drive da ultima misura
              for (j=15+1; j>8; j--)
                {
                if(warnings_motori[j-1]==1)
                  sicurezza=false;
                }  
            //inclinazione eccessiva
              for (j=3+1; j>0; j--)
                {
                if(warnings_sensori[j-1]==1)
                  sicurezza=false;
                } 
            
            //azionamento
            if((sicurezza)||(sblocco))
              {
              Wire.beginTransmission(2);  
              temp[0]=10;
              temp[1]=comando;
              temp[2]=comando2;
              Wire.write(temp,5);     // sends "10","a","b"
              stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c   
              }  
            break;

          case MOF:
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  
            Wire.write(15+comando);    
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     
            break;
            
          case MON:
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(11+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;
            
          case FAN:
            Wire.beginTransmission(1);
            Wire.write(60+comando);
            stato_i2c=Wire.endTransmission();
            if  (stato_i2c!=0) 
              digitalWrite(53,HIGH); //prob i2c
            else
              {           
              if (comando==60) stato[6][1]=false;
              if (comando==61) stato[6][1]=true;
              }
            break;  
            
          case STB:
            digitalWrite(44,HIGH);
            delay(20);
            digitalWrite(44,LOW);
            if(comando==1)
             {
             stato[6][0]=true;
             digitalWrite(51,HIGH);
             }
            if(comando==0)
             {
             stato[6][0]=false;
             digitalWrite(51,LOW); 
             }
          break; 
            
          case EMU:
            if(comando==1)
             {
             sblocco=true;
             }
            if(comando==0)
             {
             sblocco=false;
             }
          break; 
            
          case SHT:
            digitalWrite(44,HIGH);
            delay(8000);
            digitalWrite(44,LOW);
            stato[6][0]=false;
          break; 
            
          case MAF:    
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(42+comando);
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else {
              Maf=comando;
              }   
          break;
          
          case MLM:    
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(40+comando);
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else {
              Mlm=comando;
              if(Mlm){
                Tc=600;
                }
              else{
                Tc=1000;
                }
              }
          break;
          
       } 
    serInIndx  = 0;
    delay(250); //se si riduce questo ridurre anche il tempo di ricezione sulla scheda motori (attualmente 100+100)
    }
  digitalWrite(50,HIGH); //fine elaborazione comando
  }
  
  
  void serialEvent3() 
  {
    int sb; 
    int i=0;
    byte temp[10],stato_i2c;
    digitalWrite(50,LOW); //comando ricevuto
    boolean sicurezza;

    while(Serial3.available()) { 
       strcpy(serInString[0],"                   ");
       strcpy(serInString[1],"                   "); 
       strcpy(serInString[2],"                   ");
       byte cont=0;
       while (Serial3.available()){ 
          if (serInIndx>18) break; 
          sb = Serial3.read();
          serInString[0][serInIndx] = sb;
          serInIndx++;
          serInString[0][serInIndx] = -1;
          if (sb==';') 
            cont++;
          if (cont==3)
            break; 
       }
       int j=0;
       int z=0;
       while(serInString[0][j]!=';')                       //split input string in index (row 0) and command (row 1)
          j++;
       j++;
       while(serInString[0][j]!=';')
          {
          serInString[1][z]=serInString[0][j];
          j++;
          z++;
          }
       j++;
       z=0;
       while(serInString[0][j]!=';')//-1)
          {
          serInString[2][z]=serInString[0][j];
          j++;
          z++;
          }
       serInString[2][z]=';';                              //verificare se legge correttamente con o senza stop!
       int comando=atol(serInString[1]);
       int comando2;

       Serial3.println("||----Comando  BT ricevuto----||");
       j=0;
       while(serInString[0][j]!=';')                       //stampa il comando
          {
          Serial3.print(serInString[0][j]);  
          j++;
          }
       Serial3.println();
       Serial3.println(comando);
       if (serInString[0][0]!='E')
         {
         comando2=atol(serInString[2]);
         Serial3.println(comando2);
         }
       else
         {
         Serial3.println(serInString[2]);        
         }
       Serial3.println("||------------||||------------||");



       switch (serInString[0][0])
         {
          case ON:
            Wire.beginTransmission(1);  
            Wire.write(10+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c 
            break;

          case OFF:
            Wire.beginTransmission(1);  
            Wire.write(20+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;

          case CAR:
            Wire.beginTransmission(1);  
            Wire.write(30+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;

          case CA0:
            if (comando==0)
              Tc=1*1000;
            else if (comando==1)
              Tc=2*1000;
            else if (comando==2)
              Tc=5*1000;
            else if (comando==3)
              Tc=10*1000;
            else if (comando==4)
              Tc=30*1000;
            else if (comando==5)
              Tc=60*1000;
            ca0=comando;
            break;


          case CA1:  
            Wire.beginTransmission(1);  
            Wire.write(50+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            if (comando==0)             //eliminare tutto questo? forse col buffer va anche senza
              Tc_potenza=1*1000;
            else if (comando==1)
              Tc_potenza=2*1000;
            else if (comando==2)
              Tc_potenza=5*1000;
            else if (comando==3)
              Tc_potenza=10*1000;
            else if (comando==4)
              Tc_potenza=30*1000;
            else if (comando==5)
              Tc_potenza=60*1000;
            ca1=comando;
            break;  

          case CA2:
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  
            Wire.write(34+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            ca2=comando;
            break;

          case CA5:
            Wire.beginTransmission(5);  // transmit to device 5
            Wire.write(10+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            ca5=comando;
            break;

          case EXT:
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  
            j=0;
            while(serInString[2][j]!=';')                       //split input string in index (row 0) and command (row 1)
                {
                temp[j+2]=serInString[2][j];
                j++;
                }
            temp[0]=49+j;
            temp[1]=byte(comando); //motore #
            Wire.write(temp,j+2);     // sends "49+lungh","motore","...stringa..."
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
	    delay(20);
            i=0;	
            for(;;){ //infinite loop, waiting for reposnse

			if (digitalRead(9)==LOW){
                                Wire.beginTransmission(2);
				Wire.write(3);
				stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
                                delay(50);				
				Wire.requestFrom(2,32);

				while(Wire.available())    // slave may send less than requested
				{ 
					resp[i] = Wire.read();
					i++;
				}
				break; // stops infinite loop
			}
			}
            break;

          case SCA:
            Wire.beginTransmission(1);  
            Wire.write(40+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;

          case STOP:
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(20);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2cstato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;

          case R:
            if(comando==0) //master
              {
              Wire.beginTransmission(1);  
              Wire.write(20);     //OFF     si riaccende da solo
              Wire.endTransmission();
              }
            else //non master
              {
              Wire.beginTransmission(1);  
              Wire.write(20+comando);     //OFF     
              stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
              delay(Tc_potenza+500);
              Wire.beginTransmission(1);  
              Wire.write(10+comando);     //ON    
              stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
              }
            break;

          case TE:
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(27+comando);        
            Te=comando; 
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else { Te=comando; }
          break;

          case VE:    
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(21+comando);
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else Ve=comando;   
          break;

          case POI:
            pON=comando;
            break;           

          case POV:
            while (digitalRead(9)==HIGH){delay(100);}

            //controllo sicurezza
            sicurezza=true;
            //over temperature motore o drive da ultima misura
              for (j=15+1; j>8; j--)
                {
                if(warnings_motori[j-1]==1)
                  sicurezza=false;
                }  
            //inclinazione eccessiva
              for (j=3+1; j>0; j--)
                {
                if(warnings_sensori[j-1]==1)
                  sicurezza=false;
                } 

            //azionamento
            if((sicurezza)||(sblocco))
              {
              Wire.beginTransmission(2);  
              temp[0]=10;
              temp[1]=comando;
              temp[2]=comando2;
              Wire.write(temp,5);     // sends "10","a","b"
              stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c   
              }  
            break;

          case MOF:
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  
            Wire.write(15+comando);    
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     
            break;

          case MON:
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(11+comando);     // sends value byte (in base al quale lo slave decide cosa inviare)
            stato_i2c=Wire.endTransmission();if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            break;

          case FAN:
            Wire.beginTransmission(1);
            Wire.write(60+comando);
            stato_i2c=Wire.endTransmission();
            if  (stato_i2c!=0) 
              digitalWrite(53,HIGH); //prob i2c
            else
              {           
              if (comando==60) stato[6][1]=false;
              if (comando==61) stato[6][1]=true;
              }
            break;  

          case STB:
            digitalWrite(44,HIGH);
            delay(20);
            digitalWrite(44,LOW);
            if(comando==1)
             {
             stato[6][0]=true;
             digitalWrite(51,HIGH);
             }
            if(comando==0)
             {
             stato[6][0]=false;
             digitalWrite(51,LOW); 
             }
          break; 

          case EMU:
            if(comando==1)
             {
             sblocco=true;
             }
            if(comando==0)
             {
             sblocco=false;
             }
          break; 

          case SHT:
            digitalWrite(44,HIGH);
            delay(8000);
            digitalWrite(44,LOW);
            stato[6][0]=false;
          break; 

          case MAF:    
            while (digitalRead(9)==HIGH){delay(50);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(42+comando);
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else {
              Maf=comando;
              }   
          break;

          case MLM:    
            while (digitalRead(9)==HIGH){delay(100);}
            Wire.beginTransmission(2);  // transmit to device 2
            Wire.write(40+comando);
            stato_i2c=Wire.endTransmission();
            if(stato_i2c!=0) digitalWrite(53,HIGH); //prob i2c     // stop transmitting
            else {
              Mlm=comando;
              if(Mlm){
                Tc=250;
                }
              else{
                Tc=1000;
                }
              }
          break;

       } 
    serInIndx  = 0;
    delay(250); //se si riduce questo ridurre anche il tempo di ricezione sulla scheda motori (attualmente 100+100)
    }
  digitalWrite(50,HIGH); //fine elaborazione comando
  }

